%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Edited by Shu Jiang, 2011.
% ALL RIGHTS ARE RESERVED.
% mailto: shujiang@tamu.edu
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% calculate all the outputs 
% 2D with torso
% torso angles: 
%  1. ns_torso: 
%  2. s_torso;
%  3. Linearized ns_torso;
%  4. Linearized s_torso;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% addpath('/home/shu/Documents/Research_Related/Data/NewFlatGround/')
% addpath('~/Documents/Research_Related/COM_HD/')
% addpath('~/Documents/Research_Related/COM_HD/human_data_analysis/')
% addpath('~/Documents/Research_Related/COM_HD/human_data_analysis/humanData')
% addpath('/home/shu/Documents/test/K2_NTorso_HM/data')
% addpath('./Func/')
% addpath('./PlotFunc/')

addpath_output()
addpath('/home/shu/workspace/Research/output_v2/models/human/build_torso')
% name list 9 person
name  = {'Charles','Fred','Martin','Ram','Selina',... % 1st experiment
    'lily','po','ryan','vic'};  % 2nd experiment

% CONSTANT
% the number of subjects from the 1st experiment
exp1st = 5;
% the index of subjects use the right leg as swing leg during the step
rIndex = [6,7,9];
% experimental sampling frequency
fs = 480;

% initialization 
time_rec = zeros(9,1); % record time of a step
sum_weight = 0;  
sum_Lc = 0;
sum_Lt = 0;
sum_LT = 0;

% use the NAO model
% % NAO_Lc = 5137/50000;
% % NAO_Lt = 0.1;
% % NAO_LT = 18774783/130367519;
% % 
% % % Lc = NAO_Lc;
% % % Lt = NAO_Lt;
% % % LT = NAO_LT;

% % % use the amber model
% % AMBER_Lc = 17399/50000;
% % AMBER_Lt = 32639/125000;
% % AMBER_LT = 997/100000;
% % 
% %   Lc = AMBER_Lc;
% %   Lt = AMBER_Lt;
% %   LT = AMBER_LT;

for index = 1:length(name) % 5
  
    % get human data, with breaking pts for each step
    humanData = getData(name{1,index});
    
    sum_weight  = sum_weight+humanData.weight; % for average weight
    % breaking pts for each step
    StartPt = humanData.sns_brk(2);
    EndPt = humanData.sns_brk(3);
    
    % position
    if index <= exp1st
        [xpos,ypos,zpos] = getPosCartesian(humanData); % subjects from experiment 1st
    else
        [xpos,ypos,zpos] = getPosCartesianNew(humanData); % subjects from experiment 2st
    end
   
    
% %     %% use the human mean data model
% %     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % calculate length of each link
    % length of calf
    Lcl = cal_Lc(xpos.lankle_avg,ypos.lankle_avg,xpos.lknee,ypos.lknee);
    Lcr = cal_Lc(xpos.rankle_avg,ypos.rankle_avg,xpos.rknee,ypos.rknee);
    Lc = 1/2*(Lcl+Lcr);
    sum_Lc = sum_Lc+Lc;
    
    % length of thight;
    Ltl = cal_Lt(xpos.lhip,ypos.lhip,xpos.lknee,ypos.lknee);
    Ltr = cal_Lt(xpos.rhip,ypos.rhip,xpos.rknee,ypos.rknee);
    Lt = 1/2*(Ltl+Ltr);
    sum_Lt = sum_Lt+Lt;
    
    % length of torso
    LT = cal_Torso(xpos.hip_avg,ypos.hip_avg,xpos.torso,ypos.torso);
    sum_LT = sum_LT+LT;
    
% %     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    
%     DataLength = size(xpos.lknee);
    %%%%%%%%%%%  time  %%%%%%%%%%%%%%
    % scale time
    % from [c:d]
    ns_time_pre = (0:length(StartPt:EndPt)-1)'/fs;
    ns_ret = ns_time_pre;
    xpara_time = 1/ns_ret(end); % simply sacled time to 1
    ns_time = ns_ret*xpara_time;
    %%%% record time %%%%%%%%
    time_rec(index) = ns_ret(end);
    
    % other than mean human model
% %     % amber model
% %     Lc = AMBER_Lc;
% %     Lt = AMBER_Lt;
% %     LT = AMBER_LT;
    % calculate angle outputs
    if ismember(index,rIndex)
        angle = Knee2D_angle(xpos,ypos,zpos,humanData.sns_brk,2);
    else
        angle = Knee2D_angle(xpos,ypos,zpos,humanData.sns_brk,1);
    end
    
    % calculate slope outputs
    if ismember(index,rIndex)
        slope = Knee2D_slope_V2(xpos,ypos,zpos,Lc,Lt,LT,angle,humanData.sns_brk,2);
    else
        slope = Knee2D_slope_V2(xpos,ypos,zpos,Lc,Lt,LT,angle,humanData.sns_brk,1);
    end
    
    % calcualte position outputs
    if ismember(index,rIndex)
        pos = Knee2D_pos(xpos,Lc,Lt,StartPt, EndPt, angle,2);
    else    
        pos = Knee2D_pos(xpos,Lc,Lt,StartPt, EndPt, angle,1);
    end
% %     i = 1; %%%%%%%%%%%
    % calculate the mean data
    % 1. hip position
    [ave_dataHP,ave_timeHP] = BeamData(ns_time,pos.hip_pos-pos.hip_pos(1));
    ave_dataHP_s(index,:) =ave_dataHP;
%     plot(ave_dataHP); hold on;


    % 2. linearized hip position
    [ave_dataHPL,ave_timeHPL] = BeamData(ns_time,pos.hip_pos_Linearized-pos.hip_pos_Linearized(1));
    test = pos.hip_pos_Linearized-pos.hip_pos_Linearized(1);
    test(1)
    ave_dataHPL_s(index,:) =ave_dataHPL;
%     plot(ave_dataHPL); hold on;


    % 3. non-stance slope
    [ave_dataNSslope,ave_timeNSslope] = BeamData(ns_time,slope.nsslope);
    ave_dataNSslope_s(index,:) =ave_dataNSslope;
%     plot(ave_dataNSslope); hold on;

    % 4. linearized non-stance slope
    [ave_dataNSslopeL,ave_timeNSslopeL] = BeamData(ns_time,slope.Linearized_nsslope);
    ave_dataNSslopeL_s(index,:) =ave_dataNSslopeL; 
%     plot(ave_dataNSslopeL); hold on;
    
    % 5. hip angle
    [ave_dataHip,ave_timeHip] = BeamData(ns_time,angle.hip);
    ave_dataHip_s(index,:) =ave_dataHip;  
%     plot(ave_dataHip); hold on;
    
    % 6. stance knee
    [ave_dataSK,ave_timeSK] = BeamData(ns_time,angle.sknee);
    ave_dataSK_s(index,:) = ave_dataSK;
%     plot(ave_dataSK); hold on;
    
    % 7. non-stance knee
    [ave_dataNSK,ave_timeNSK] = BeamData(ns_time,angle.nsknee);
    ave_dataNSK_s(index,:) = ave_dataNSK;
%         plot(ave_dataNSK); hold on;
        
    % 8. stance torso slope
    [ave_dataST,ave_timeST] = BeamData(ns_time,slope.storso);
    ave_dataST_s(index,:) = ave_dataST;
%     plot(ave_dataST,'r'); hold on;
    
    % 9. Linearized stance torso slope
    [ave_dataSTL,ave_timeSTL] = BeamData(ns_time,slope.Linearized_storso);
    ave_dataSTL_s(index,:) = ave_dataSTL;
%     plot(ave_dataSTL); hold on;
    
    
    % 10. non-stance torso slope
    [ave_dataNST,ave_timeNST] = BeamData(ns_time,slope.nstorso);
    ave_dataNST_s(index,:) = ave_dataNST;
%      plot(ave_dataNST,'r'); hold on;
    
    % 11. Linearized non-stance torso slope
    [ave_dataNSTL,ave_timeNSTL] = BeamData(ns_time,slope.Linearized_nstorso);
    ave_dataNSTL_s(index,:) = ave_dataNSTL;
    
    % 12. torso hip angle
    [ave_dataTH,ave_timeTH] = BeamData(ns_time,angle.torso_hip);
    ave_dataTH_s(index,:) = ave_dataTH;
    
    % 13. theta4
    [ave_dataT4,ave_timeT4] = BeamData(ns_time,angle.theta4);
    ave_dataT4_s(index,:) = ave_dataT4;
    
    % 14. theta3
    [ave_dataT3,ave_timeT4] = BeamData(ns_time,angle.theta3);
    ave_dataT3_s(index,:) = ave_dataT3; 
    
    % 15. linearized stance slope
    [ave_dataSslopeL,ave_timeSslopeL] = BeamData(ns_time,slope.Linearized_sslope);
    ave_dataLSslope_s(index,:) = ave_dataSslopeL; 
    
    % 16. stance ankle angle
    [ave_dataSA,ave_timeSA] = BeamData(ns_time,angle.sa);
    ave_dataSA_s(index,:) = ave_dataSA;   
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    q = [angle.sa;angle.sknee;angle.theta3;angle.theta4;angle.nsknee];
    for ind = 1:size(q,2)
        com(ind) = comFn(q(:,ind));
        delta_com(ind) = deltaCOMFn(q(:,ind));     
    end
    
    % 17. COM
    [ave_dataCOM,ave_timeCOM] = BeamData(ns_time,com);
    ave_dataCOM_s(index,:) = ave_dataCOM; 
%     
%     
     % 18. stance ankle angle
    [ave_dataCOML,ave_timeCOML] = BeamData(ns_time,delta_com);
    ave_dataCOML_s(index,:) = ave_dataCOML; 
    
    
%     q = [ave_dataSA;ave_dataSK;ave_dataT3;ave_dataT4;ave_dataNSK];

% COM for NAO
% % for ind = 1:size(q,2)
% %     NAO_com(ind) = NAO_pcom_sca(q(:,ind),  0, 0);
% %     NAO_delta_com(ind) = NAO_deltapcom_sca(q(:,ind),0,0);
% % end
% %     NAO_com_s(index,:) = NAO_com;
%plot(ave_dataNSTL); hold on;

% plot(angle.theta3); hold on;
    com = [];
    delta_com = [];
    
end
% [x_meanCOM,upperBCOM,lowerBCOM] = meanValue(NAO_com_s);

%% calculate the mean value and save 
    % 1. hip position
    [x_meanHP,upperBHP,lowerBHP] = meanValue(ave_dataHP_s);

    % 2. linearized hip position
    [x_meanHPL,upperBHPL,lowerBHPL] = meanValue(ave_dataHPL_s);

    % 3. non-stance slope
    [x_meanNSL,upperBNSL,lowerBNSL] = meanValue(ave_dataNSslope_s);

    % 4. linearized non-stance slope
    [x_meanLNS,upperBLNS,lowerBLNS] = meanValue(ave_dataNSslopeL_s);
%     plot(ave_dataNSslopeL); hold on;
    
    % 5. hip angle
    [x_meanHip,upperBHip,lowerBHip] = meanValue(ave_dataHip_s);
    
    % 6. stance knee
     [x_meanSK,upperBSK,lowerBSK] = meanValue(ave_dataSK_s);
%     plot(ave_dataSK); hold on;
    
    % 7. non-stance knee
   [x_meanNSK,upperBNSK,lowerBNSK] = meanValue(ave_dataNSK_s);
%         plot(ave_dataNSK); hold on;
        
    % 8. stance torso slope
     [x_meanST,upperBST,lowerBST]= meanValue(ave_dataST_s);
    
%     plot(ave_dataST,'r'); hold on;
    
    % 9. Linearized stance torso slope
    [x_meanLST,upperBLST,lowerBLST] = meanValue(ave_dataSTL_s);
%     plot(ave_dataSTL); hold on;
    
    
    % 10. non-stance torso slope
    [x_meanNST,upperBNST,lowerBNST] = meanValue(ave_dataNST_s);
%      plot(ave_dataNST,'r'); hold on;
    
    % 11. Linearized non-stance torso slope
    [x_meanLNST,upperBLNST,lowerBLNST]=meanValue(ave_dataNSTL_s);
    
    % 12. torso hip angle
    [x_meanTH,upperBTH,lowerBTH]=meanValue(ave_dataTH_s);
    
    % 13. theta4
    [x_meanT4,upperBT4,lowerBT4]=meanValue(ave_dataT4_s);
    
    % 14. theta3
    [x_meanT3,upperBT3,lowerBT3]=meanValue(ave_dataT3_s);
    
    
    % 15. linearized stance slope
    [x_meanLS,upperBLS,lowerBLS]=meanValue(ave_dataLSslope_s);
    
    % 16. stance ankle angle
    [x_meanSA,upperBSA,lowerBSA]=meanValue(ave_dataSA_s);
    
    % 17. COM
     [x_meanCOM,upperBCOM,lowerBCOM]=meanValue(ave_dataCOM_s);
     
     % 18. delta COM
     [x_meanCOML,upperBCOML,lowerBCOML]=meanValue(ave_dataCOML_s);
%% save data
    ns_time = mean(time_rec)*ave_timeSK'; % averaged time
    data_name = 'mean_data';
%     data_name = 'mean_data';
% 1. hip angle 
hip_angle =x_meanHip';
save(strcat('dataTorso/',data_name,'_hipAngle.mat'),'hip_angle','ns_time')

% 2. hip position 
hip_pos = x_meanHP';
save(strcat('dataTorso/',data_name,'_hippos_shift.mat'),'hip_pos','ns_time');

% 3. non-stance knee angle
ns_knee = x_meanNSK';
save(strcat('dataTorso/',data_name,'_nsknee.mat'),'ns_knee','ns_time');

% 4. stance knee angle
s_knee = x_meanSK';
save(strcat('dataTorso/',data_name,'_sknee.mat'),'s_knee','ns_time');

% 5. non-stance slope
ns_slope = x_meanNSL';
save(strcat('dataTorso/',data_name,'_nsslope.mat'),'ns_slope','ns_time');

% 6. linearized non-stance slope
Linear_nsslope = x_meanLNS';
save(strcat('dataTorso/',data_name,'_LinearNSslope.mat'),'Linear_nsslope','ns_time');

% 7. Linearized hip position 
Linear_hippos = x_meanHPL';

% fittedmodel1 = fit(ns_time, Linear_hippos, 'poly1');
% a1 = coeffvalues(fittedmodel1)
% 
% Linear_hippos = Linear_hippos-a1(2);
save(strcat('dataTorso/',data_name,'_LinearHippos.mat'),'Linear_hippos','ns_time')

% 8. stance torso slope
s_torso = x_meanST';
save(strcat('dataTorso/',data_name,'_storso.mat'),'s_torso','ns_time');
% 9. linearized stance torso slope
Linear_s_torso = x_meanLST';
save(strcat('dataTorso/',data_name,'_LinearStorso.mat'),'Linear_s_torso','ns_time');
% 10. non-stance torso slope
ns_torso=x_meanNST';
save(strcat('dataTorso/',data_name,'_nstorso.mat'),'ns_torso','ns_time');
% 11. linearized non-stance torso slope
Linear_ns_torso = x_meanLNST';
save(strcat('dataTorso/',data_name,'_LinearNStorso.mat'),'Linear_ns_torso','ns_time');

torso_angle =x_meanTH';
save(strcat('dataTorso/',data_name,'_TorsoHipAngle.mat'),'torso_angle','ns_time');

theta4 = x_meanT4';
save(strcat('dataTorso/',data_name,'_theta4.mat'),'theta4','ns_time');

theta3 = x_meanT3';
save(strcat('dataTorso/',data_name,'_theta3.mat'),'theta3','ns_time');

s_slope = x_meanLS;
save(strcat('dataTorso/',data_name,'_sslope.mat'),'s_slope','ns_time');


% 16 stance ankle angle
s_ankle = x_meanSA';
save(strcat('dataTorso/',data_name,'_sankle.mat'),'s_ankle','ns_time');

% 17 COM
com = x_meanCOM';
save(strcat('dataTorso/',data_name,'_com.mat'),'com','ns_time');

% 18 linearized COM
Linear_com = x_meanCOML';
save(strcat('dataTorso/',data_name,'_LinearCOM.mat'),'Linear_com','ns_time');